/*
  Copyright (c) 2010 Les Newell. All rights reserved/

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/



//Modules don't need a #define guard block because they are only loaded once

#include "altimeter.h"
#include "spi.h"
#include "multifilter.h"

#define MAX_PRESS 120000
#define MIN_PRESS 30000

#define SCP_SMOOTH 8
#define TIMEOUT 100
#define SAMPLE_AVERAGE 16 //Average ground level over this many samples

//Addresses
#define REVID 0x00  //ASIC Revision Number
#define OPERATION 0x03  //Operation register
#define OPSTATUS 0x04   //Operation Status
#define STATUS 0x07     //ASIC Status
#define START 0x0A      //Constant Readings
#define PRESSURE 0x1F   //Pressure 3 MSB
#define PRESSURE_LSB 0x20 //Pressure 16 LSB
#define TEMP 0x21       //16 bit temp

#define OP_STOP 0x00
#define OP_READIND 0x01
#define OP_WRITEIND 0x02
#define OP_READEEPROM 0x05
#define OP_WRITEEEPROM 0x06
#define OP_INIT 0x07
#define OP_HISPEED 0x09
#define OP_HIRES 0x0a
#define OP_LOWPOWER 0x0b
#define OP_SINGLE 0x0c
#define OP_SELFTEST 0x0f

DEFINE_MODULE(SCP1000,Altimeter)

public:
    SCP1000()
    {
        m_timeout = TIMEOUT;
        m_altitude = 0;
        m_gain = -8.24;
        m_filter.zeros[0] = 0;

        AddSocket("SCP1000.Filter",m_filter.amounts);
        AddSocket("SCP1000.Gain",&m_gain);
    }


//perform your initialization here
    virtual void Init()
    {
        pinMode(SCPSELECT,OUTPUT);
        digitalWrite(SCPSELECT,HIGH); //disable device
        pinMode(SCPDRDY, INPUT);
        SetInterval(30);
        SetPriority(10);
        delay(100);
        Reset();
    }

    virtual void Reference()
    {
       m_filter.Reference(SAMPLE_AVERAGE);
    }

    virtual void ParamChanged(Socket * param)
    {
        m_filter.ParamChanged(param);
    }

//This function is called at the rate specified by SetInterval
    virtual void Loop(const unsigned long& interval)
    {
        if (!Ready())
        {
            return;
        }

        long int press = ReadPressure();
        if (press > MAX_PRESS || press < MIN_PRESS)
        {
//      m_error |= ERROR_RANGE;
            return;
        }
        float alt = press * m_gain;
        m_filter.Update(&alt);
        altitude = m_filter.filtered[0];
    }

private:
    void Reset()
    {
        SPI.Write(OPERATION,OP_STOP);
        if (digitalRead(SCPDRDY) == LOW)
        {
            ReadPressure(); //dummy read to clear last data
        }
        int timeout = 600; //max 600ms
        while (SPI.Read(OPSTATUS) != 0)
        {
            delay(1);
            if (timeout-- < 0)
            {

            }
        };
        SPI.Write(OPERATION,OP_HISPEED);

    }

    long int ReadPressure()
    {
        long int ret;
        ret = (SPI.Read(PRESSURE) & B00000111);
        ret = ret << 16;
        ret |= SPI.Read16(PRESSURE_LSB);
        ret = ret /4;
        return(ret);
    }

    bool Ready()
    {
        if (digitalRead(SCPDRDY) == LOW)
        {
            if ((--m_timeout) == 0) //sensor is not responding
            {
                Reset();
                m_timeout = TIMEOUT;
            }
            return(false);
        }
        m_timeout = TIMEOUT;
        return(true);
    }

    int m_timeout;
    long int m_altitude;
    ParameterF m_gain;
    MultiFilterRef<1> m_filter;

};

SCP1000 g_SCP1000;


